package org.nashua.tt151;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.SimpleRobot;
import java.io.IOException;
import org.nashua.tt151.libraries.Controller.DualAction;

public class HopperTester extends SimpleRobot {
    DualAction joy = new DualAction(1);
    DigitalInput forwardSwitch = new DigitalInput(1);
    DigitalInput backwardSwitch = new DigitalInput(2);
    AnalogChannel potentiometer = new AnalogChannel(1);
    Jaguar arm = new Jaguar(1);
    Dash dash = null;
    private int failedCount;
    private boolean tryingToConnect = false;
    private boolean jammed = false;
    private double pos;
    private boolean back;
    private double power;
    private void connectToDashboard() {
        if (tryingToConnect) {
            return;
        }
        tryingToConnect = true;
        new Thread() {
            public void run() {
                try {
                    if (tryingToConnect && dash==null) {
                        dash = new Dash("10.1.53.5", new Dash.ConnectionListener() {
                            public void onConnect() {
                                System.out.println("[Connected]");
                            }
                            public void onDisconnect() {
                                System.out.println("[Disconnected]");
                                dash = null;
                            }
                            public void onDataReceived(String msg) {
                                System.out.println("[MSG "+msg+"]");
                            }
                        });
                    }
                } catch (IOException ex) {
                    System.err.println("[ERR Failed to connect to dashboard: "+ex.getMessage()+"]");
                }
                tryingToConnect = false;
            }
        }.start();
    }
    public void disabled() {
        if (dash==null && !tryingToConnect) {
            connectToDashboard();
        }
    }
    public void robotInit() {
        connectToDashboard();
        failedCount = 0;
        new java.util.Timer().scheduleAtFixedRate(new java.util.TimerTask(){
            public void run() {
                if (dash!=null) {
                    try {
                        dash.sendAnalog(potentiometer.getAverageValue(), "Feeder Pot", potentiometer.getChannel(), Dash.AnalogType.POTENTIOMETER);
                        dash.sendDigitalIO(forwardSwitch.get(), "Forward SW", forwardSwitch.getChannel());
                        dash.sendDigitalIO(backwardSwitch.get(), "Back SW", backwardSwitch.getChannel());
                        dash.sendPWM((int)(arm.get()*128+128), "Feeder Arm", arm.getModuleNumber(), arm.getChannel(), Dash.PWMType.JAGUAR);
                        dash.logHopperStatus(jammed);
                        failedCount = 0;
                    } catch (Exception ex) {
                        System.err.println("Failed to update dashboard: "+ex.getMessage());
                        failedCount++;
                        if (failedCount<=10) {
                            failedCount = 0;
                            dash = null;
                            connectToDashboard();
                        }
                    }
                } else {
                    failedCount = 0;
                    connectToDashboard();
                }
            }
        }, 1, 100);
    }
    public void operatorControl() {
        pos = 0.0;
        back = false;
        power = 0.0;
        while (isEnabled() && isOperatorControl()) {
            joy.queryButtons();
            pos = 0;
            if (joy.wasReleased(DualAction.Button.LEFT_TRIGGER)) {
                power -= 0.10;
            } else if (joy.wasReleased(DualAction.Button.LEFT_BUMPER)) {
                power -= 0.01;
            } else if (joy.wasReleased(DualAction.Button.RIGHT_TRIGGER)) {
                power += 0.10;
            } else if (joy.wasReleased(DualAction.Button.RIGHT_BUMPER)) {
                power += 0.01;
            }
            power = Math.max(0, Math.min(1, power));
            if (joy.getRawButton(DualAction.Button.Y)) {
                long start = System.currentTimeMillis();
                while (true) {
                    if (!back) {
                        if (!forwardSwitch.get() && (System.currentTimeMillis()-start<2000)) {
                            pos = power;
                        } else {
                            start = System.currentTimeMillis();
                            back = true;
                        }
                    } else {
                        if (!backwardSwitch.get() && (System.currentTimeMillis()-start<2000)) {
                            pos = -power;
                        } else {
                            back = false;
                            pos = 0.0;
                            break;
                        }
                    }
                    arm.set(pos);
                }
            } else {
                arm.set(0);
            }
        }
    }
}
